/*
 * GStreamer
 * Copyright (C) 2005 Thomas Vander Stichele <thomas@apestaart.org>
 * Copyright (C) 2005 Ronald S. Bultje <rbultje@ronald.bitfreak.net>
 * Copyright (C) 2010 snegovick <<user@hostname.org>>
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 *
 * Alternatively, the contents of this file may be used under the
 * GNU Lesser General Public License Version 2.1 (the "LGPL"), in
 * which case the following provisions apply instead of the ones
 * mentioned above:
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Library General Public License for more details.
 *
 * You should have received a copy of the GNU Library General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 */

/**
 * SECTION:element-ecvthreshold
 *
 * FIXME:Describe ecvthreshold here.
 *
 * <refsect2>
 * <title>Example launch line</title>
 * |[
 * gst-launch -v -m fakesrc ! ecvthreshold ! fakesink silent=TRUE
 * ]|
 * </refsect2>
 */

#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif

#include <gst/gst.h>
#include <string.h>

#include "gstecvthreshold.h"

#include <stdio.h>
#include <math.h>
#include <stdlib.h>

GST_DEBUG_CATEGORY_STATIC (gst_ecv_threshold_debug);
#define GST_CAT_DEFAULT gst_ecv_threshold_debug

/* Filter signals and args */
enum
{
  /* FILL ME */
  LAST_SIGNAL
};

enum
{
  PROP_0,
  PROP_SILENT,
  PROP_KERNELSIZE,
  PROP_SIGMA
};

/* the capabilities of the inputs and outputs.
 *
 * describe the real formats here.
 */
static GstStaticPadTemplate sink_factory = GST_STATIC_PAD_TEMPLATE ("sink",
    GST_PAD_SINK,
    GST_PAD_ALWAYS,
    GST_STATIC_CAPS ("video/x-raw-rgb")
    );

static GstStaticPadTemplate src_factory = GST_STATIC_PAD_TEMPLATE ("src",
    GST_PAD_SRC,
    GST_PAD_ALWAYS,
    GST_STATIC_CAPS ("video/x-raw-rgb")
    );

GST_BOILERPLATE (GstecvThreshold, gst_ecv_threshold, GstElement,
    GST_TYPE_ELEMENT);

static void gst_ecv_threshold_set_property (GObject * object, guint prop_id,
    const GValue * value, GParamSpec * pspec);
static void gst_ecv_threshold_get_property (GObject * object, guint prop_id,
    GValue * value, GParamSpec * pspec);

static gboolean gst_ecv_threshold_set_caps (GstPad * pad, GstCaps * caps);
static GstFlowReturn gst_ecv_threshold_chain (GstPad * pad, GstBuffer * buf);

/* GObject vmethod implementations */

static void
gst_ecv_threshold_base_init (gpointer gclass)
{
  GstElementClass *element_class = GST_ELEMENT_CLASS (gclass);

  gst_element_class_set_details_simple(element_class,
    "ecvThreshold",
    "FIXME:Generic",
    "FIXME:Generic Template Element",
    "snegovick <<user@hostname.org>>");

  gst_element_class_add_pad_template (element_class,
      gst_static_pad_template_get (&src_factory));
  gst_element_class_add_pad_template (element_class,
      gst_static_pad_template_get (&sink_factory));
}

static GstStateChangeReturn
gst_ecv_threshold_change_state (GstElement *element, GstStateChange transition);


/* initialize the ecvthreshold's class */
static void
gst_ecv_threshold_class_init (GstecvThresholdClass * klass)
{
  GObjectClass *gobject_class;
  GstElementClass *gstelement_class;

  gobject_class = (GObjectClass *) klass;
  gstelement_class = (GstElementClass *) klass;

  gstelement_class->change_state = gst_ecv_threshold_change_state;

  gobject_class->set_property = gst_ecv_threshold_set_property;
  gobject_class->get_property = gst_ecv_threshold_get_property;

  g_object_class_install_property (gobject_class, PROP_SILENT,
      g_param_spec_boolean ("silent", "Silent", "Produce verbose output ?",
          FALSE, G_PARAM_READWRITE));
  g_object_class_install_property (gobject_class, PROP_KERNELSIZE,
                                   g_param_spec_uint ("kernelsize", "Kernel size", "Size of gaussian filter kernel", 0, UINT_MAX, 3, G_PARAM_READWRITE));
  g_object_class_install_property (gobject_class, PROP_SIGMA,
                                   g_param_spec_float ("sigma", "Sigma", "Sigma parameter", 0.0f, 1.0f, 0.5f, G_PARAM_READWRITE));

}

/* initialize the new element
 * instantiate pads and add them to element
 * set pad calback functions
 * initialize instance structure
 */
static void
gst_ecv_threshold_init (GstecvThreshold * filter,
    GstecvThresholdClass * gclass)
{
  filter->sinkpad = gst_pad_new_from_static_template (&sink_factory, "sink");
  gst_pad_set_setcaps_function (filter->sinkpad,
                                GST_DEBUG_FUNCPTR(gst_ecv_threshold_set_caps));
  gst_pad_set_getcaps_function (filter->sinkpad,
                                GST_DEBUG_FUNCPTR(gst_pad_proxy_getcaps));
  gst_pad_set_chain_function (filter->sinkpad,
                              GST_DEBUG_FUNCPTR(gst_ecv_threshold_chain));

  filter->srcpad = gst_pad_new_from_static_template (&src_factory, "src");
  gst_pad_set_getcaps_function (filter->srcpad,
                                GST_DEBUG_FUNCPTR(gst_pad_proxy_getcaps));

  gst_element_add_pad (GST_ELEMENT (filter), filter->sinkpad);
  gst_element_add_pad (GST_ELEMENT (filter), filter->srcpad);
  filter->silent = FALSE;
  filter->kernelsize = 3;
  filter->sigma = 0.5f;
}

static void
gst_ecv_threshold_set_property (GObject * object, guint prop_id,
    const GValue * value, GParamSpec * pspec)
{
  GstecvThreshold *filter = GST_ECVTHRESHOLD (object);

  switch (prop_id) {
    case PROP_SILENT:
      filter->silent = g_value_get_boolean (value);
      break;
    case PROP_KERNELSIZE:
      filter->kernelsize = g_value_get_uint (value);
      break;
    case PROP_SIGMA:
      filter->sigma = g_value_get_float (value);
      break;
    default:
      G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
      break;
  }
}

static void
gst_ecv_threshold_get_property (GObject * object, guint prop_id,
    GValue * value, GParamSpec * pspec)
{
  GstecvThreshold *filter = GST_ECVTHRESHOLD (object);

  switch (prop_id) {
    case PROP_SILENT:
      g_value_set_boolean (value, filter->silent);
      break;
    case PROP_KERNELSIZE:
      g_value_set_uint (value, filter->kernelsize);
      break;
    case PROP_SIGMA:
      g_value_set_float (value, filter->sigma);
      break;
    default:
      G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
      break;
  }
}

/* GstElement vmethod implementations */

/* this function handles the link with other elements */
static gboolean
gst_ecv_threshold_set_caps (GstPad * pad, GstCaps * caps)
{
  GstecvThreshold *filter;
  GstPad *otherpad;
  GstStructure *structure = gst_caps_get_structure (caps, 0);
  const gchar *mime;

  filter = GST_ECVTHRESHOLD (gst_pad_get_parent (pad));
  otherpad = (pad == filter->srcpad) ? filter->sinkpad : filter->srcpad;

  /* Since we're an audio filter, we want to handle raw audio
   * and from that audio type, we need to get the samplerate and
   * number of channels. */
  mime = gst_structure_get_name (structure);
  if (strcmp (mime, "video/x-raw-rgb") != 0) 
  {
    GST_WARNING ("Wrong mimetype %s provided, we only support %s",
		 mime, "video/x-raw-rgb");
  }
  gst_structure_get_int (structure, "width", &filter->width);
  gst_structure_get_int (structure, "height", &filter->height);
  
  gst_object_unref (filter);
  
  return gst_pad_set_caps (otherpad, caps);
}

void * getp(unsigned int psize, unsigned int w, unsigned int h, unsigned int x, unsigned int y, void * buf)
{
  if (x>w || x<0 || y>h || y<0)
    return NULL;
  return buf + (psize*w*y+x*psize);
}

int fcalc_kernel(unsigned int size, float *kernel, float sigma)
{
  int i = 0;
  int j = 0;
  float * ptr;
  float sigma2 = sigma*sigma;
  for (;i<size;i++)
  {
    for (j=0;j<size;j++)
    {
      ptr = getp(sizeof(float), size, size, i, j, kernel);
      if (ptr != NULL)
        *ptr = expf(-(pow(ceil(i-size/2.0), 2.0)+pow(floor(size/2.0-j), 2.0))/(2.0*sigma2))/(2.0*(float)M_PI*sigma2);
    }
  }
  
  return size;
}

GstBuffer * applygauss (unsigned int w, unsigned int h, float * kernel, unsigned int kernelsize, char * img)
{
  int i = 0;
  int j = 0;
  int k = 0;
  GstBuffer *buffer;
  gint size;
  size = w * h * 4;
  buffer = gst_buffer_new ();
  GST_BUFFER_SIZE (buffer) = size;
  GST_BUFFER_MALLOCDATA (buffer) = g_malloc (size);
  GST_BUFFER_DATA (buffer) = GST_BUFFER_MALLOCDATA (buffer);

  unsigned char * dataout = GST_BUFFER_DATA(buffer);
  unsigned char * optr;
  unsigned char * iptr;
  float * mptr;
  unsigned char c;
  unsigned char d;
  for (;i<h;i++)
  {
    for (j=0;j<w;j++)
    {
      optr = getp(4, w, h, j, i, dataout);

      for (k=0;k<kernelsize;k++)
      {
        iptr = getp(4, w, h, j-kernelsize/2+k, i, img);
        mptr = getp(sizeof(float), kernelsize, kernelsize, k, kernelsize/2, kernel);
        if (optr && iptr && mptr)
        {
          d = *optr;
          c = (unsigned char)*iptr;
          *optr = d + (c)*(*mptr);
        }
      }
    }
  }
  for (i=0;i<h;i++)
  {
    for (j=0;j<w;j++)
    {
      optr = getp(4, w, h, j, i, dataout);

      for (k=0;k<kernelsize;k++)
      {
        iptr = getp(4, w, h, j, i-kernelsize/2+k, img);
        mptr = getp(sizeof(float), kernelsize, kernelsize, kernelsize/2, k, kernel);
        if (optr && iptr && mptr)
        {
          d = *optr;
          c = (unsigned char)*iptr;
          *optr = d + (c)*(*mptr);
        }
      }
    }
  }

  return buffer;
}


/* chain function
 * this function does the actual processing
 */
static GstFlowReturn
gst_ecv_threshold_chain (GstPad * pad, GstBuffer * buf)
{
  GstecvThreshold *filter;

  filter = GST_ECVTHRESHOLD (GST_OBJECT_PARENT (pad));

  GstBuffer * wb = gst_buffer_make_writable(buf);
  
  int sz = 0;
  if ((sz = GST_BUFFER_SIZE(wb)) != 0)
  {
    unsigned char * data = GST_BUFFER_DATA(wb);
    GstBuffer * b = applygauss(filter->width, filter->height, filter->kernel, filter->kernelsize, data);

    gst_buffer_copy_metadata(b, buf, GST_BUFFER_COPY_ALL);
    gst_buffer_unref(buf);
    return gst_pad_push (filter->srcpad, b);
  }
  else
    GST_WARNING ("zero sized buffer");

  /* just push out the incoming buffer without touching it */
  return gst_pad_push (filter->srcpad, buf);
}


/* entry point to initialize the plug-in
 * initialize the plug-in itself
 * register the element factories and other features
 */
static gboolean
ecvthreshold_init (GstPlugin * ecvthreshold)
{
  /* debug category for fltering log messages
   *
   * exchange the string 'Template ecvthreshold' with your description
   */
  GST_DEBUG_CATEGORY_INIT (gst_ecv_threshold_debug, "ecvthreshold",
      0, "Template ecvthreshold");

  return gst_element_register (ecvthreshold, "ecvthreshold", GST_RANK_NONE,
      GST_TYPE_ECVTHRESHOLD);
}

/* PACKAGE: this is usually set by autotools depending on some _INIT macro
 * in configure.ac and then written into and defined in config.h, but we can
 * just set it ourselves here in case someone doesn't use autotools to
 * compile this code. GST_PLUGIN_DEFINE needs PACKAGE to be defined.
 */
#ifndef PACKAGE
#define PACKAGE "myfirstecvthreshold"
#endif

/* gstreamer looks for this structure to register ecvthresholds
 *
 * exchange the string 'Template ecvthreshold' with your ecvthreshold description
 */
GST_PLUGIN_DEFINE (
    GST_VERSION_MAJOR,
    GST_VERSION_MINOR,
    "ecvthreshold",
    "Template ecvthreshold",
    ecvthreshold_init,
    VERSION,
    "LGPL",
    "GStreamer",
    "http://gstreamer.net/"
)

gboolean 
gst_ecv_threshold_allocate_memory (GstecvThreshold * filter)
{
  filter->kernel= NULL;
  if (filter->kernelsize%2 == 0)
    filter->kernelsize++;
  
  filter->kernel = malloc(sizeof(float)*filter->kernelsize*filter->kernelsize);

  fcalc_kernel(filter->kernelsize, filter->kernel, filter->sigma);

  return TRUE;
}

gboolean 
gst_ecv_threshold_free_memory (GstecvThreshold * filter)
{
  free(filter->kernel);
  return TRUE;
}

static GstStateChangeReturn
gst_ecv_threshold_change_state (GstElement *element, GstStateChange transition)
{
  GstStateChangeReturn ret = GST_STATE_CHANGE_SUCCESS;
  GstecvThreshold *filter = GST_ECVTHRESHOLD (element);

  switch (transition) {
    case GST_STATE_CHANGE_NULL_TO_READY:
      //printf ("null to ready\n");
      if (!gst_ecv_threshold_allocate_memory (filter))
        return GST_STATE_CHANGE_FAILURE;
      break;
    default:
      break;
  }

  ret = GST_ELEMENT_CLASS (parent_class)->change_state (element, transition);
  if (ret == GST_STATE_CHANGE_FAILURE)
    return ret;

  switch (transition) {
    case GST_STATE_CHANGE_READY_TO_NULL:
      printf ("ready to null\n");
      gst_ecv_threshold_free_memory (filter);
      break;
    default:
      break;
  }

  return ret;
}

